#!/usr/bin/env python3
from geometry_msgs.msg import PoseStamped, PoseWithCovarianceStamped
from rclpy.node import Node
import rclpy

class BasicNavigator(Node):
    def __init__(self, node_name='basic_navigator'):
        super().__init__(node_name)
        
        # 初始化位姿
        self.initial_pose = PoseStamped()
        self.initial_pose.header.frame_id = 'map'
        # 创建初始位姿发布器
        self.initial_pose_pub = self.create_publisher(
            PoseWithCovarianceStamped,
            'initialpose',
            10
        )
        self.get_logger().info("导航器初始化完成")

    def setInitialPose(self, pose):
        """
        设置初始位姿
        :param pose: geometry_msgs/PoseStamped
        """
        self.initial_pose = pose
        
        # 发布初始位姿
        initial_pose_msg = PoseWithCovarianceStamped()
        initial_pose_msg.header = pose.header
        initial_pose_msg.pose.pose = pose.pose
        self.initial_pose_pub.publish(initial_pose_msg)
        
        self.get_logger().info(f"已设置初始位姿: x={pose.pose.position.x:.2f}, y={pose.pose.position.y:.2f}, w={pose.pose.orientation.w:.2f}")

    def waitUntilNav2Active(self, timeout_sec=10):
        
        # 等待导航系统就绪（简化版）直接等待一秒
        # 实际实现应该检查各个服务是否可用

        self.get_logger().info("等待导航系统初始化...")
        # 这里简化实现，实际应该检查生命周期节点状态
        rclpy.spin_once(self, timeout_sec=1)
        self.get_logger().info("导航系统已就绪")

    def destroy(self):
        """清理资源"""
        self.destroy_node()